#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav_msgs/msg/odometry.hpp"

class O2P : public rclcpp::Node
{
	rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub;
	rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub;
	std::string input_topic;
	std::string output_topic;
	nav_msgs::msg::Path path_out;
	public:
	void odom_callback(nav_msgs::msg::Odometry msg)
	{
		path_out.header.frame_id = msg.header.frame_id;
		path_out.header.stamp = msg.header.stamp;
		geometry_msgs::msg::PoseStamped pose;
		pose.header = msg.header;
		pose.pose = msg.pose.pose;
		path_out.poses.push_back(pose);
		path_pub->publish(path_out);
	}

	O2P(std::string name) : Node(name)
	{
		this->declare_parameter("input_topic", "odom");
		this->get_parameter("input_topic", input_topic);
		this->declare_parameter("output_topic", "path");
		this->get_parameter("output_topic", output_topic);
		odom_sub = this->create_subscription<nav_msgs::msg::Odometry>(input_topic, 100, std::bind(&O2P::odom_callback, this, std::placeholders::_1));
		path_pub = this->create_publisher<nav_msgs::msg::Path>(output_topic, 100);
	}

};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<O2P>("odom_to_path");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}


